/* lcd_driver.c */
#include "LCD.h"
#include <stdarg.h>
#include <stdio.h>
#include <string.h>
#include "logger.h"

// 私有函数声明
static void lcd_write(LCD_HandleTypeDef *hlcd, uint8_t data, uint8_t rs_mode);
static void write_4bit(LCD_HandleTypeDef *hlcd, uint8_t data);
static void write_8bit(LCD_HandleTypeDef *hlcd, uint8_t data);

LCD_Error LCD_Init(LCD_HandleTypeDef *hlcd, LCD_ConfigTypeDef *config) {
    if (!hlcd || !config) return LCD_ERR_PARAM;

    memcpy(&hlcd->config, config, sizeof(LCD_ConfigTypeDef));
    
    // 初始化GPIO
    hlcd->config.RS.set_output();
    hlcd->config.RW.set_output();
    hlcd->config.EN.set_output();
    for (int i = 0; i < hlcd->config.data_bits; i++) {
        hlcd->config.DATA[i].set_output();
    }

    // 初始化序列
    hlcd->config.delay_ms(50);
    uint8_t function_set = 0x20 | ((hlcd->config.display_lines > 1) << 3);
    function_set |= (hlcd->config.data_bits == 8) << 4;

    for (int i = 0; i < 3; i++) {
        write_8bit(hlcd, function_set | 0x30);
        hlcd->config.delay_ms(5);
    }

    // 设置工作模式
    lcd_write(hlcd, function_set, 0);
    hlcd->config.delay_ms(1);

    // 显示控制初始化
    hlcd->display_control = 0x0C; // 显示开，无光标
    lcd_write(hlcd, 0x08 | hlcd->display_control, 0);

    LCD_Clear(hlcd);
    hlcd->entry_mode = 0x02; // 增量模式
    lcd_write(hlcd, 0x04 | hlcd->entry_mode, 0);

    return LCD_OK;
}

static void lcd_write(LCD_HandleTypeDef *hlcd, uint8_t data, uint8_t rs_mode) {
    rs_mode ? hlcd->config.RS.set_high(0) : hlcd->config.RS.set_low(0);
    hlcd->config.RW.set_low(0);

    log_message(LOG_INFO, "LCD Write: RS = %d, RW = 0, Data = 0x%02X", rs_mode, data);

    if (hlcd->config.data_bits == 8) {
        write_8bit(hlcd, data);
    } else {
        write_4bit(hlcd, data);
    }
}

static void write_8bit(LCD_HandleTypeDef *hlcd, uint8_t data) {
    for (int i = 0; i < 8; i++) {
        (data & (1 << i)) ? hlcd->config.DATA[i].set_high(i)
                          : hlcd->config.DATA[i].set_low(i);
    }
    log_message(LOG_INFO, "EN set high");
    hlcd->config.EN.set_high(0);
    hlcd->config.delay_us(1);
    log_message(LOG_INFO, "EN set low");
    hlcd->config.EN.set_low(0);
    hlcd->config.delay_us(100);
}

static void write_4bit(LCD_HandleTypeDef *hlcd, uint8_t data) {
    // 高四位
    for (int i = 4; i < 8; i++) {
        (data & (1 << i)) ? hlcd->config.DATA[i-4].set_high(i)
                          : hlcd->config.DATA[i-4].set_low(i);
    }
    log_message(LOG_INFO, "EN set high (high nibble)");
    hlcd->config.EN.set_high(0);
    hlcd->config.delay_us(1);
    log_message(LOG_INFO, "EN set low (high nibble)");
    hlcd->config.EN.set_low(0);
    hlcd->config.delay_us(100);

    // 低四位
    for (int i = 0; i < 4; i++) {
        (data & (1 << i)) ? hlcd->config.DATA[i].set_high(i)
                          : hlcd->config.DATA[i].set_low(i);
    }
    log_message(LOG_INFO, "EN set high (low nibble)");
    hlcd->config.EN.set_high(0);
    hlcd->config.delay_us(1);
    log_message(LOG_INFO, "EN set low (low nibble)");
    hlcd->config.EN.set_low(0);
    hlcd->config.delay_us(100);
}

LCD_Error LCD_Clear(LCD_HandleTypeDef *hlcd) {
    lcd_write(hlcd, 0x01, 0);
    hlcd->config.delay_ms(2);
    hlcd->cursor_row = 0;
    hlcd->cursor_col = 0;
    return LCD_OK;
}

LCD_Error LCD_SetCursor(LCD_HandleTypeDef *hlcd, uint8_t row, uint8_t col) {
    const uint8_t row_offsets[] = {0x40, 0x00};
    if (row >= hlcd->config.display_lines || col >= 16) return LCD_ERR_PARAM;
    
    lcd_write(hlcd, 0x80 | (col + row_offsets[row]), 0);
    hlcd->cursor_row = row;
    hlcd->cursor_col = col;
    //setCursorPosition(col, row);
    log_message(LOG_INFO, "Set cursor to row %d, col %d", row, col); 
    return LCD_OK;
}

LCD_Error LCD_Print(LCD_HandleTypeDef *hlcd, const char *str) {
    while (*str) {
        lcd_write(hlcd, *str++, 1);
        if (++hlcd->cursor_col >= 16) {
            hlcd->cursor_col = 0;
            if (++hlcd->cursor_row >= hlcd->config.display_lines) {
                hlcd->cursor_row = 0;
            }
            log_message(LOG_INFO, "New line, row %d, col %d", hlcd->cursor_row, hlcd->cursor_col);
        }
    }
    return LCD_OK;
}

LCD_Error LCD_Printf(LCD_HandleTypeDef *hlcd, uint8_t row, uint8_t col, const char *fmt, ...) {
    char buf[33];
    va_list args;
    va_start(args, fmt);
    vsnprintf(buf, sizeof(buf), fmt, args);
    va_end(args);
    log_message(LOG_INFO, "LCD Printf: %s", buf);
    LCD_SetCursor(hlcd, row, col);
    return LCD_Print(hlcd, buf);
}

LCD_Error LCD_DisplayOn(LCD_HandleTypeDef *hlcd) {
    if (!hlcd) return LCD_ERR_PARAM;
    hlcd->display_control |= 0x04;
    lcd_write(hlcd, hlcd->display_control, 0);
    return LCD_OK;
}

#include <windows.h>
void setCursorPosition(int x, int y) {
    COORD coord;
    coord.X = x;
    coord.Y = y;
    HANDLE hConsole = GetStdHandle(STD_OUTPUT_HANDLE);
    SetConsoleCursorPosition(hConsole, coord);
}